#include <front_end.hpp>


// 传送左目右目图像que
std::mutex g_picLeftLock;
std::queue<sensor_msgs::ImageConstPtr> g_picLeftQue;
std::mutex g_picRightLock;
std::queue<sensor_msgs::ImageConstPtr> g_picRightQue;

// 传送给后端的

Eigen::Vector2d cv2f2Eigen2d(cv::Point2f & in){
    return Eigen::Vector2d(in.x, in.y);
}


FeatureTracker::FeatureTracker(){}
FeatureTracker::FeatureTracker(std::shared_ptr<ReadParameter> inParma){
    // 传递参数指针
    mspParam = inParma;
    // 初始化orb
    mspORBextractor = std::shared_ptr<EASY::ORBextractor>(new EASY::ORBextractor(mspParam->nFeatures,
                                                                            mspParam->fScaleFactor,
                                                                            mspParam->nLevels,
                                                                            mspParam->fIniThFAST,
                                                                            mspParam->fMinThFAST));


}
FeatureTracker::~FeatureTracker(){}

// 前端线程运行：提取orb特征点并计算描述子
void FeatureTracker::run(){
    
    std::cout << "Begin Feature Tracker" << std::endl;
    while(1){
        
        sensor_msgs::ImageConstPtr rosLImageptr = nullptr;
        sensor_msgs::ImageConstPtr rosRImageptr = nullptr;
        g_picLeftLock.lock();
        if(g_picLeftQue.size() > 0){
            rosLImageptr = g_picLeftQue.front();
            g_picLeftQue.pop();
        }
        g_picLeftLock.unlock();
        if(rosLImageptr == nullptr)
            continue;

        g_picRightLock.lock();
        if(g_picRightQue.size() > 0){
            rosRImageptr = g_picRightQue.front();
            g_picRightQue.pop();
        }
        g_picRightLock.unlock();
        if(rosRImageptr == nullptr)
            continue;

        // ros2cv
        cv::Mat picLeft;
        rosImg2opencvMat_color(rosLImageptr, picLeft);
        cv::Mat picRight;
        rosImg2opencvMat_color(rosRImageptr, picRight);

        // 图像转灰度图
        if(picLeft.channels()==3)
        {
            if(false)
            {
                cvtColor(picLeft,picLeft,CV_RGB2GRAY);
                cvtColor(picRight,picRight,CV_RGB2GRAY);
            }
            else
            {
                cvtColor(picLeft,picLeft,CV_BGR2GRAY);
                cvtColor(picRight,picRight,CV_BGR2GRAY);
            }
        }
        // 构建 left frame
        Frame* leftFrame = new Frame(picLeft, rosLImageptr->header.stamp.toSec(),mspORBextractor);
        leftFrame->calculateOrb();
        // 构建 right frame
        Frame* rightFrame = new Frame(picRight, rosRImageptr->header.stamp.toSec(),mspORBextractor);
        rightFrame->calculateOrb();

        // Todo：View
        if(mEnableShowFeature){
            std::vector<cv::KeyPoint> & keyPoints = leftFrame->mvKeys;
            // draw point
            cv::Mat showMat;
            picLeft.copyTo(showMat);// 深拷贝
            for(int i = 0; i < keyPoints.size(); i++){
                cv::circle(showMat, cv::Point(keyPoints[i].pt.x, keyPoints[i].pt.y), 3, cv::Scalar(0, 0, 255), 1);
            }
            std::cout << "frame id:" << leftFrame->mlId << "feature num =" << leftFrame->mvKeys.size() << std::endl;
            // show
            cv::namedWindow("pic", cv::WINDOW_AUTOSIZE);
            cv::imshow("pic", showMat);
            cv::waitKey(1);
        }

        // run mode
        if(mspParam->runMode == "build"){// in build mode
            computationalStereo();
        }
        // 发送给后端：只发送左目的特征点（恢复深度的）、rgbd只发送特征深度图丢弃  
        
    }
}
// 双目计算距离 need camera param
void FeatureTracker::computationalStereo(){
    // 匹配

    // 计算
    
}
// 从rgbd相机中恢复特征点深度
void FeatureTracker::computationalRGBD(){

}

// 是否在图像内
bool inBorder(const cv::Point2f &pt)
{
    const int BORDER_SIZE = 1;
    int img_x = cvRound(pt.x);
    int img_y = cvRound(pt.y);
    return BORDER_SIZE <= img_x && img_x < COL - BORDER_SIZE && BORDER_SIZE <= img_y && img_y < ROW - BORDER_SIZE;
}

// 去除非统一状态点--后面用模板类重新写一下
void reduceVector(std::vector<cv::Point2f> &v, std::vector<uchar> status)
{
    int j = 0;
    for (int i = 0; i < int(v.size()); i++)
        if (status[i])
            v[j++] = v[i];
    v.resize(j);
}

void reduceVector(std::vector<int> &v, std::vector<uchar> status)
{
    int j = 0;
    for (int i = 0; i < int(v.size()); i++)
        if (status[i])
            v[j++] = v[i];
    v.resize(j);
}

void reduceVector(std::vector<std::pair<long long, int>> &v, std::vector<uchar> status)
{
    int j = 0;
    for (int i = 0; i < int(v.size()); i++)
        if (status[i])
            v[j++] = v[i];
    v.resize(j);
}


